package com.googlecode.grs.robot;

import java.awt.geom.Point2D;
import java.awt.geom.Point2D.Double;

import com.googlecode.grs.core.GRTObject;
import com.googlecode.grs.core.Mechanism;
import com.googlecode.grs.environment.SimulationEnvironment;

/**
 * A DiffentialDrivenBot integrates its position given the velocities of two
 * wheels.
 * 
 * This class serves to abstract Wheel methods, as well as serve as a 'frame' by
 * continuously calculating overall heading.
 * 
 * Once the robot intersects a physical object, it will drive out of it and
 * halt.
 * 
 * @author ajc
 * 
 */
public class DifferentialDrivenBot extends GRTObject implements Moveable,
		Mechanism {

	public static final double DISTANCE_BETWEEN_WHEELS = 20;

	private final Wheel left;
	private final Wheel right;
	private double heading;

	private boolean running = false;

	private final SimulationEnvironment env;

	/**
	 * Constructs a new robot in 2D space and orientation
	 * 
	 * @param env
	 *            abstraction of world, to prevent robot collisions with
	 *            physical objects
	 * @param x
	 *            x component of 2D position
	 * @param y
	 *            y component of 2D position
	 * @param heading
	 *            2D orientation
	 */

	public DifferentialDrivenBot(SimulationEnvironment env, double x, double y,
			double heading) {
		this.env = env;
		this.heading = heading;
		left = createLeft(this, x, y, heading);
		right = createRight(this, x, y, heading);
	}

	/**
	 * Commands the bot to drive directly given wheel velocities
	 * 
	 * @param leftSpeed
	 *            velocity for the left wheel to turn
	 * @param rightSpeed
	 *            velocity for the right wheel to turn
	 */
	public void drive(double leftSpeed, double rightSpeed) {
		left.setVelocity(leftSpeed);
		right.setVelocity(rightSpeed);
	}

	/**
	 * Starts wheel and heading position integration
	 */
	public void startIntegration() {
		left.startIntegration();
		right.startIntegration();
		start();
	}

	/**
	 * Calculates wheel heading
	 */
	public void run() {
		running = true;
		while (running) {
			// recalculate heading
			heading = getRobotHeading(left.getX(), left.getY(), right.getX(),
					right.getY());
			// backOutFromWall();// back out of wall if necessary
			correctWheels();
			try {
				sleep(1);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}

	}

	/**
	 * repositions the bot's wheels so that the distance between them is
	 * constant
	 */
	public void correctWheels() {
		Wheel left = getLeftWheel();
		Wheel right = getRightWheel();
		double centerX = getX();
		double centerY = getY();
		double angle = getPointAngle(left.getX(), left.getY(), right.getX(),
				right.getY());
		double robotRadius = DISTANCE_BETWEEN_WHEELS / 2;
		left.setPosition(centerX + robotRadius * Math.cos(angle), centerY
				+ robotRadius * Math.sin(angle));
		right.setPosition(centerX - robotRadius * Math.cos(angle), centerY
				- robotRadius * Math.sin(angle));
	}

	/**
	 * Gets robot heading given the 2D positions of 2 wheels
	 * 
	 * @param leftX
	 * @param leftY
	 * @param rightX
	 * @param rightY
	 * @return TODO document
	 */
	private static double getRobotHeading(double leftX, double leftY,
			double rightX, double rightY) {
		return getPointAngle(leftX, leftY, rightX, rightY) + Math.PI / 2;
	}

	/**
	 * Gets the angle between 2 points
	 * 
	 * @param leftX
	 * @param leftY
	 * @param rightX
	 * @param rightY
	 * @return angle TODO document
	 * 
	 */
	private static double getPointAngle(double leftX, double leftY,
			double rightX, double rightY) {
		double x = leftX - rightX;
		double y = leftY - rightY;
		double angle = Math.atan(y / x);
		if (x < 0) {
			angle += Math.PI;
		}
		return angle;
	}

	/**
	 * Checks if robot is in a wall, and while in a wall drive in reverse to get
	 * out from it, and stop at the end.
	 */
	private void backOutFromWall() {

		// check if wheres the mouse isHittingWall
		// if true reverse velocity
		if (robotisHittingWall()) {
			left.setVelocity(-left.getVelocity());
			right.setVelocity(-right.getVelocity());
			// while in a wall, wait

			while (robotisHittingWall()) {
				try {
					Thread.sleep(2);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			// once out of wall, halt
			halt();
		}

	}

	/**
	 * 
	 * @return true if the robot is hitting a wall.
	 */
	private boolean robotisHittingWall() {
		for (int i = 0; i < env.getObjects().size(); i++) {
			if (env.get(i).intersects(getX(), getY())) {
				return true;

			}
		}
		return false;
	}

	@Override
	/**
	 * Takes an average of the x components of wheel position
	 * @return robot's x component of position
	 */
	public double getX() {
		return (left.getX() + right.getX()) / 2.0;
	}

	@Override
	/**
	 * Takes an average of the y components of wheel position
	 * @return robot's y component of position
	 */
	public double getY() {
		return (left.getY() + right.getY()) / 2.0;
	}

	@Override
	/**
	 * Finds robot heading given the 2D positions of each wheel
	 * @return heading, or orientation in 2D space
	 */
	public double getHeading() {
		return heading;
	}

	/**
	 * Accesses left wheel hardware
	 * 
	 * @return left wheel
	 */
	public Wheel getLeftWheel() {
		return left;
	}

	/**
	 * Accesses right wheel hardware
	 * 
	 * @return right wheel
	 */
	public Wheel getRightWheel() {
		return right;
	}

	@Override
	public void setPosition(double x, double y, double angle) {
		// TODO Auto-generated method stub

	}

	@Override
	public void setPosition(Double position, double angle) {
		// TODO Auto-generated method stub

	}

	@Override
	public void pause() {
		// TODO Auto-generated method stub

	}

	@Override
	public void unPause() {
		// TODO Auto-generated method stub

	}

	@Override
	public void halt() {
		left.halt();
		right.halt();
	}

	private static Wheel createLeft(DifferentialDrivenBot bot, double x,
			double y, double heading) {
		// subtract 90 deg. from offset due to difference between robot heading
		// and actual angle between wheels
		double xstart = x + (DISTANCE_BETWEEN_WHEELS / 2)
				* Math.cos(heading - Math.PI / 2);
		double ystart = y + (DISTANCE_BETWEEN_WHEELS / 2)
				* Math.sin(heading - Math.PI / 2);
		return new Wheel(bot, xstart, ystart);

	}

	private static Wheel createRight(DifferentialDrivenBot bot, double x,
			double y, double heading) {
		// add 90 deg. from offset due to difference between robot heading
		// and actual angle between wheels
		double xstart = x + (DISTANCE_BETWEEN_WHEELS / 2)
				* Math.cos(heading + Math.PI / 2);
		double ystart = y + (DISTANCE_BETWEEN_WHEELS / 2)
				* Math.sin(heading + Math.PI / 2);

		return new Wheel(bot, xstart, ystart);

	}

	/**
	 * Test method, verification for wheel correction
	 * 
	 * @param left
	 * @param right
	 * @return
	 */
	private static double getDistanceBetweenWheels(Wheel left, Wheel right) {
		Point2D.Double leftPoint = new Point2D.Double(left.getX(), left.getY());
		Point2D.Double rightPoint = new Point2D.Double(right.getX(),
				right.getY());
		return leftPoint.distance(rightPoint);
	}

}
